#include "../includes.h"

extern void Start_Position_Survey(char mode);
extern void Survey_Position();
extern void Init_POSITION();
extern void Init_POSITION2();

void Survey_Position();
void Init_POSITION();
void Init_POSITION2();

void Init_POSITION()
{

    /* SYSTEM->POSITION */
    SYSTEM.POSITION.AveragingCounter 	= 0;
    SYSTEM.POSITION.PAveragingCounter 	= 0;
    SYSTEM.POSITION.Survey 				= SURVEY_OFF;

    SYSTEM.POSITION.CURRENT.LAT  = 0;
    SYSTEM.POSITION.CURRENT.LON  = 0;
    SYSTEM.POSITION.CURRENT.HEI  = 0;

    SYSTEM.POSITION.LASTHELD 	= SYSTEM.POSITION.CURRENT;
	SYSTEM.POSITION.AVERAGE 	= SYSTEM.POSITION.CURRENT;
	SYSTEM.POSITION.PAVERAGE 	= SYSTEM.POSITION.CURRENT;
	SYSTEM.POSITION.ACC 		= SYSTEM.POSITION.CURRENT;

 	SYSTEM.POSITION.AveragingNo = 60*60;
 	//SYSTEM.POSITION.AveragingNo = 60*3;
 	//SYSTEM.POSITION.AveragingNo = 30;
}
void Init_POSITION2()	// OEMV1-Rcv-Data		sewon
{

    /* SYSTEM->POSITION */
    SYSTEM.POSITION2.AveragingCounter 	= 0;
    SYSTEM.POSITION2.PAveragingCounter 	= 0;
    SYSTEM.POSITION2.Survey 				= SURVEY_OFF;

    SYSTEM.POSITION2.CURRENT.LAT  = 0;
    SYSTEM.POSITION2.CURRENT.LON  = 0;
    SYSTEM.POSITION2.CURRENT.HEI  = 0;

    SYSTEM.POSITION2.LASTHELD 	= SYSTEM.POSITION2.CURRENT;
	SYSTEM.POSITION2.AVERAGE 	= SYSTEM.POSITION2.CURRENT;
	SYSTEM.POSITION2.PAVERAGE 	= SYSTEM.POSITION2.CURRENT;
	SYSTEM.POSITION2.ACC 		= SYSTEM.POSITION2.CURRENT;

 	SYSTEM.POSITION2.AveragingNo = 60*60;
 	//SYSTEM.POSITION2.AveragingNo = 60*3;
 	//SYSTEM.POSITION2.AveragingNo = 30;
}


void Start_Position_Survey(char mode)
{
	if(mode == 0){
    	SYSTEM.POSITION.AveragingCounter 	= 0;
    	SYSTEM.POSITION.PAveragingCounter 	= 0;
    }

	SYSTEM.POSITION.Survey 				= SURVEY_ON;
    SYSTEM.POSITION.PositionMode		= POSITION_MODE_AVERAGING;
}


void Survey_Position()
{

/*
	SYSTEM.EB.pstatus = SURVEY_IN_MODE;
	SYSTEM.EB.pstatus = TIME_TRANSFER_MODE;
	SYSTEM.EB.pstatus = NO_POSITION;
*/
    //if( !((SYSTEM.TRACK.DOPTYPE == 0) && ((SYSTEM.TRACK.STATUS>>5)&0x01)) ){
    if( !(!(SYSTEM.TRACK.DOPTYPE&0x01) && ((SYSTEM.TRACK.STATUS>>5)&0x01)) ){
		return;
    }
    
    if(CONFIG.AntCheckFlag){
    	if(SYSTEM.EB.ANTENNA != OK) {return;}
    }

	if(SYSTEM.POSITION.Survey == SURVEY_OFF) 	return;


    switch(SYSTEM.POSITION.PAveragingCounter){

    	case	0:
					SYSTEM.POSITION.ACC.LAT = 0;
					SYSTEM.POSITION.ACC.LON = 0;
					SYSTEM.POSITION.ACC.HEI = 0;

    				SYSTEM.POSITION.PAVERAGE = SYSTEM.POSITION.ACC;
    				SYSTEM.POSITION.AVERAGE = SYSTEM.POSITION.ACC;

    				SYSTEM.POSITION.PAveragingCounter++;
    				break;
    	case	1:
    	case	2:
    	case	3:
	   				SYSTEM.POSITION.PAVERAGE.LAT += SYSTEM.POSITION.CURRENT.LAT;
    				SYSTEM.POSITION.PAVERAGE.LON += SYSTEM.POSITION.CURRENT.LON;
    				SYSTEM.POSITION.PAVERAGE.HEI += SYSTEM.POSITION.CURRENT.HEI;
    				SYSTEM.POSITION.PAveragingCounter++;
    				break;
    	case	4:
			    	SYSTEM.POSITION.PAVERAGE.LAT /= 3;
			    	SYSTEM.POSITION.PAVERAGE.LON /= 3;
			    	SYSTEM.POSITION.PAVERAGE.HEI /= 3;

			    	SYSTEM.POSITION.PAveragingCounter = 100;
    				break;
    }


    if( SYSTEM.POSITION.PAveragingCounter != 100 )    	return;

	SYSTEM.POSITION.AveragingCounter++;

	SYSTEM.POSITION.ACC.LAT += (SYSTEM.POSITION.CURRENT.LAT - SYSTEM.POSITION.PAVERAGE.LAT);
	SYSTEM.POSITION.ACC.LON += (SYSTEM.POSITION.CURRENT.LON - SYSTEM.POSITION.PAVERAGE.LON);
	SYSTEM.POSITION.ACC.HEI += (SYSTEM.POSITION.CURRENT.HEI - SYSTEM.POSITION.PAVERAGE.HEI);

	SYSTEM.POSITION.AVERAGE.LAT = SYSTEM.POSITION.ACC.LAT/SYSTEM.POSITION.AveragingCounter + SYSTEM.POSITION.PAVERAGE.LAT;
	SYSTEM.POSITION.AVERAGE.LON = SYSTEM.POSITION.ACC.LON/SYSTEM.POSITION.AveragingCounter + SYSTEM.POSITION.PAVERAGE.LON;
    SYSTEM.POSITION.AVERAGE.HEI = SYSTEM.POSITION.ACC.HEI/SYSTEM.POSITION.AveragingCounter + SYSTEM.POSITION.PAVERAGE.HEI;

	//if( 0 && SYSTEM.POSITION.AveragingCounter == SYSTEM.POSITION.AveragingNo ){
	if( SYSTEM.POSITION.AveragingCounter == SYSTEM.POSITION.AveragingNo ){
        SYSTEM.POSITION.AveragingCounter = 0;
        SYSTEM.POSITION.PAveragingCounter = 0;
        SYSTEM.POSITION.Survey = SURVEY_OFF;

		SYSTEM.EB.PHP.POSITION = SYSTEM.POSITION.AVERAGE;
        // � Position Hold Event  ��̵Ǹ�
        //     SYSTEM.POSITION.PositionMode = POSITION_MODE_KNOWN  �° ȴ�
        SYSTEM.EB.PHP_Enable = ON;
        log_pos_hold1();
        log_pos_hold2();
	}
	
	if(SYSTEM.POSITION.AveragingCounter >= SYSTEM.POSITION.AveragingNo) 
		SYSTEM.POSITION.AveragingCounter = SYSTEM.POSITION.AveragingNo;

/*

	if( SYSTEM.IO.CTRL_PORT.MODE == UDEBUG){

		sprintf((char*)MESG,"%d %d %ld %ld %ld  %ld %ld %ld  %ld %ld %ld\r\n",	SYSTEM.POSITION.AveragingCounter,SYSTEM.POSITION.PAveragingCounter,
						SYSTEM.POSITION.AVERAGE.LAT,SYSTEM.POSITION.AVERAGE.LON,SYSTEM.POSITION.AVERAGE.HEI,
						SYSTEM.POSITION.ACC.LAT,SYSTEM.POSITION.ACC.LON,SYSTEM.POSITION.ACC.HEI,
						SYSTEM.POSITION.CURRENT.LAT,SYSTEM.POSITION.CURRENT.LON,SYSTEM.POSITION.CURRENT.HEI);

		TransmitMessage((BYTE*)MESG,PORT_CONTROL);
	}
*/
}




